/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/publicInterfaceInc/struct_pid.h
 * @Description  : 定义了PID控制器和LQR参数的结构体，用于AGV导航系统中的控制算法。
 *                 包括PID参数的设置和LQR参数的读取，便于在控制过程中进行参数配置和调整。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:20:30
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include <string>

// PID filter - float type
struct PidFilter {
    float kp;    // 比例系数
    float ki;    // 积分系数
    float kd;    // 微分系数
    float il;    // 积分饱和限

    PidFilter() : kp(0.0f), ki(0.0f), kd(0.0f), il(0.0f) {}
};

// pid参数单元
struct PidUnit {
    PidFilter pid_y;  // 横向PID参数
    PidFilter pid_a;  // 角度PID参数
    float speed;      // 当前参数启用阈值速度

    PidUnit() : speed(0.5f) {}
};

// pid参数单元
struct PidGroup {
  bool use;
  bool cycle_read;
  float wheel_base;
  PidUnit low;
  PidUnit high;

  PidGroup() : use(false), cycle_read(false), wheel_base(1.0f) {}
};

// LQR参数读取
struct LqrParamReader {
    bool use;
    bool cycle_read;
    double qx, qy, qtheta, rv, rw, prepoint_dist, eps;
    int iterations;
    double lqr_threshold;

    LqrParamReader()
        : use(false),
        cycle_read(false),
        qx(1.0),
        qy(1.0),
        qtheta(1.0),
        rv(1.0),
        rw(1.0),
        prepoint_dist(2.0),
        eps(0.01),
        iterations(50),
        lqr_threshold(0.9) {}
};
